#include "motor_controller.h"
#include "motor_driver.h"

MotorController motorController[MOTOR_NUM];
         
int32_t dSpeed = 0;
         
/**
 * @brief  电机控制器初始化函数
 * @param  None
 * @retval None
 */
void MotorController_Init(void) 
{	
    
	for(uint8_t i=0;i<MOTOR_NUM;i++){
        motorController[i].accelerate=MOTORCONTROLLER_ACC;
        motorController[i].setSpeed=0;
        motorController[i].curSpeed=0;
        motorController[i].PID.output=0;                   // PWM_DUTY_LIMIT/2
        
        motorController[i].PID.pidParam.kp=MOTORCONTROLLER_KP;
        motorController[i].PID.pidParam.ki=MOTORCONTROLLER_KI;
        motorController[i].PID.pidParam.kd=MOTORCONTROLLER_KD;
        
        motorController[i].PID.outMINLimit= 0;         // 0
        motorController[i].PID.outMAXLimit= PWM_DUTY_LIMIT;         // PWM_DUTY_LIMIT
        
        motorController[i].PID.lastError = 0;
        
        //位置式
        motorController[i].PID.FilterPercent = 0.7;
        motorController[i].PID.iTerm = 0;
        motorController[i].PID.integrationLimit = 9000;
        
    }
}

/**
 * @brief  电机速度设置函数
 * @param  nMotor 电机编号
 * @param  nSpeed 设定的速度值,单位:转/分钟
 * @retval None
 */
void SetMotorSpeed(uint8_t nMotor, int16_t nSpeed){ 
    motorController[nMotor].setSpeed = nSpeed;
}

/**
 * @brief  更新电机当前速度
 * @param  None
 * @retval None
 */
void UpdateMotorCurSpeed(void){
    uint16_t EncodeValue;
    int32_t speed; 
    for(uint8_t i=0;i<MOTOR_NUM;i++){
        EncodeValue = Encoder_GetValue(i);
        if(EncodeValue!=0){
            speed = 3000000 * 60 / EncodeValue / 9;
        }else{
            speed = 0;
        }
        motorController[i].curSpeed = speed;   
    }
    static int32_t lastSpeed = 0 ;
    dSpeed = (speed - lastSpeed)*1000/MOTOR_CONTROLLER_PERIOD;
    lastSpeed = speed;
}

/**
 * @brief  电机周期控制任务
 * @param  任务控制码
 * @retval 任务状态
 */
uint8_t MotorControlTask(uint8_t info){

    UpdateMotorCurSpeed();
    
    for(uint8_t i=0;i<MOTOR_NUM;i++){
        
        if(motorController[i].setSpeed>motorController[i].curSpeed){      
            motorController[i].expSpeed+=motorController[i].accelerate*MOTOR_CONTROLLER_PERIOD/1000;

            if(motorController[i].expSpeed>motorController[i].setSpeed)
                motorController[i].expSpeed=motorController[i].setSpeed;
        }
        else if(motorController[i].setSpeed<motorController[i].curSpeed){
            motorController[i].expSpeed-=motorController[i].accelerate*MOTOR_CONTROLLER_PERIOD/1000;

            if(motorController[i].expSpeed<motorController[i].setSpeed)
                motorController[i].expSpeed=motorController[i].setSpeed;
        }
        else motorController[i].expSpeed=motorController[i].curSpeed;
        
        //调用PID控制器
        //INCPID_Update(&motorController[i].PID, motorController[i].expSpeed,motorController[i].curSpeed);
        POSPID_Update( &motorController[i].PID , motorController[i].expSpeed , motorController[i].curSpeed , 0.01);

        //更新PWM输出
        Motor_SetPWMDuty(i,motorController[i].PID.output);
    }
    
    return 0;
}

